using BepuUtilities;
using System.Runtime.CompilerServices;
using System;
using System.Diagnostics;
using BepuUtilities.Memory;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.Trees
{
    partial struct Tree
    {
        /// <summary>
        /// 将新的叶节点与现有的叶节点合并,生成引用这两个叶的新内部节点,然后返回该叶节点的索引。
        /// </summary>
        /// <param name="newLeafBound">要添加的叶的边框。</param>
        /// <param name="parentIndex">现有叶所属的父节点的索引。</param>
        /// <param name="indexInParent">子节点与现有叶所属的父节点的索引。</param>
        /// <param name="merded">包含新叶和现有叶的边框。</param>
        /// <return>叶的索引</return>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe int MergeLeafNodes(ref BoundingBox newLeafBounds, int parentIndex, int indexInParent, ref BoundingBox merged)
        {
            // 它是一个叶节点。
            // 使用新叶和旧叶作为子节点创建新的内部节点。
            // 这是唯一有可能创建新关卡的地方。

            var newNodeIndex = AllocateNode();
            ref var newNode = ref Nodes[newNodeIndex];
            ref var newMetanode = ref Metanodes[newNodeIndex];
            newMetanode.Parent = parentIndex;
            newMetanode.IndexInParent = indexInParent;
            newMetanode.RefineFlag = 0;
            // 新节点的第一个子节点是旧叶。插入其边框。
            ref var parentNode = ref Nodes[parentIndex];
            ref var childInParent = ref Unsafe.Add(ref parentNode.A, indexInParent);
            newNode.A = childInParent;

            // 将新叶插入第二个子槽中。
            ref var b = ref newNode.B;
            b.Min = newLeafBounds.Min;
            var leafIndex = AddLeaf(newNodeIndex, 1);
            b.Index = Encode(leafIndex);
            b.Max = newLeafBounds.Max;
            b.LeafCount = 1;

            // 用新的索引信息更新旧的叶节点。
            var oldLeafIndex = Encode(newNode.A.Index);
            Leaves[oldLeafIndex] = new Leaf(newNodeIndex, 0);

            // 更新原始节点的子指针和边界框。
            childInParent.Index = newNodeIndex;
            childInParent.Min = merged.Min;
            childInParent.Max = merged.Max;
            Debug.Assert(childInParent.LeafCount == 1);
            childInParent.LeafCount = 2;
            return leafIndex;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        unsafe int InsertLeafIntoEmptySlot(ref BoundingBox leafBox, int nodeIndex, int childIndex, ref Node node)
        {
            var leafIndex = AddLeaf(nodeIndex, childIndex);
            ref var child = ref Unsafe.Add(ref node.A, childIndex);
            child.Min = leafBox.Min;
            child.Index = Encode(leafIndex);
            child.Max = leafBox.Max;
            child.LeafCount = 1;
            return leafIndex;
        }
        enum BestInsertionChoice
        {
            NewInternal,
            Traverse
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        static void CreateMerged(ref Vector3 minA, ref Vector3 maxA, ref Vector3 minB, ref Vector3 maxB, out BoundingBox merged)
        {
            merged.Min = Vector3.Min(minA, minB);
            merged.Max = Vector3.Max(maxA, maxB);
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static unsafe BestInsertionChoice ComputeBestInsertionChoice(ref BoundingBox bounds, float newLeafCost, ref NodeChild child, out BoundingBox mergedCandidate, out float costChange)
        {
            CreateMerged(ref child.Min, ref child.Max, ref bounds.Min, ref bounds.Max, out mergedCandidate);
            var newCost = ComputeBoundsMetric(ref mergedCandidate);
            if (child.Index >= 0)
            {
                // 将子节点展开的成本估计为max(sah(NewLeafBound),costChange)*log2(Child.LeafCount)。
                // 我们假设剩余的树是平衡的,并且每个级别将至少扩展SAH(NewLeafBound)。
                // 这可能并不接近正确,但这不是一个糟糕的估计。
                costChange = newCost - ComputeBoundsMetric(ref child.Min, ref child.Max);
                costChange += SpanHelper.GetContainingPowerOf2(child.LeafCount) * Math.Max(newLeafCost, costChange);
                return BestInsertionChoice.Traverse;
            }
            else
            {
                costChange = newCost;
                return BestInsertionChoice.NewInternal;
            }

        }

        /// <summary>
        /// 将叶子添加到具有给定边界框的树中,并返回添加的叶子的索引。
        /// </summary>
        /// <param name="bound">叶边界的范围。</param>
        /// <param name="pool">需要调整大小时使用的资源池。</param>
        /// <return>树的叶数组中分配的叶的索引。</return>
        public unsafe int Add(ref BoundingBox bounds, BufferPool pool)
        {
            // 函数的睡觉假设我们有足够的空间。我们不想在添加过程中处理无效的指针。
            if (Leaves.Length == leafCount)
            {
                // 请注意,当我们添加1时,底层池将请求可以容纳它的下一个更高的2次方(以字节为单位)。
                // 既然我们已经满负荷了,那将是原来的两倍。
                Resize(pool, leafCount + 1);
            }

            // 假设：索引0始终是根(如果存在),并且空树将具有子计数为0的"根"。
            int nodeIndex = 0;
            var newLeafCost = ComputeBoundsMetric(ref bounds);
            while (true)
            {
                // 树叶应该属于哪个孩子？

                // 将叶分配给成本变化最小的任何节点。
                ref var node = ref Nodes[nodeIndex];
                // 这是一棵二叉树,所以一个节点只有在它是根节点时才能拥有不满的子节点。
                // 按照惯例,空树仍然有一个没有子节点的根节点,因此我们必须处理这种情况。
                if (leafCount < 2)
                {
                    // 因为填充空槽的启发式成本的变化为零,所以最好的槽将与将其插入叶节点捆绑在一起。
                    return InsertLeafIntoEmptySlot(ref bounds, nodeIndex, leafCount, ref node);
                }
                else
                {
                    ref var a = ref node.A;
                    ref var b = ref node.B;
                    var choiceA = ComputeBestInsertionChoice(ref bounds, newLeafCost, ref a, out var mergedA, out var costChangeA);
                    var choiceB = ComputeBestInsertionChoice(ref bounds, newLeafCost, ref b, out var mergedB, out var costChangeB);
                    if (costChangeA <= costChangeB)
                    {
                        if (choiceA == BestInsertionChoice.NewInternal)
                        {
                            return MergeLeafNodes(ref bounds, nodeIndex, 0, ref mergedA);
                        }
                        else // IF(choiceA==BestInsertionChoice.Traverse)
                        {
                            a.Min = mergedA.Min;
                            a.Max = mergedA.Max;
                            nodeIndex = a.Index;
                            ++a.LeafCount;
                        }
                    }
                    else
                    {
                        if (choiceB == BestInsertionChoice.NewInternal)
                        {
                            return MergeLeafNodes(ref bounds, nodeIndex, 1, ref mergedB);
                        }
                        else // IF(choiceB==BestInsertionChoice.Traverse)
                        {
                            b.Min = mergedB.Min;
                            b.Max = mergedB.Max;
                            nodeIndex = b.Index;
                            ++b.LeafCount;
                        }
                    }
                }


            }
        }


        [MethodImpl(MethodImplOptions.NoInlining)]
        internal static float ComputeBoundsMetric(ref BoundingBox bounds)
        {
            return ComputeBoundsMetric(ref bounds.Min, ref bounds.Max);
        }
        [MethodImpl(MethodImplOptions.NoInlining)]
        internal static float ComputeBoundsMetric(ref Vector3 min, ref Vector3 max)
        {
            // 请注意,我们只使用SAH。虽然我们主要对碰撞检测的体积查询感兴趣,但是拓扑差异
            // 在体积启发式和表面积启发式之间并不是很大。然而,体积启发式算法遇到了一个令人恼火的大问题：
            // 一个范围等于零的所有边界框都具有零成本。表面积法简单地避免了这个洞。
            var offset = max - min;
            // 请注意,这仅与表面积成正比。被一个恒定的因素缩放是无关紧要的。
            return offset.X * offset.Y + offset.Y * offset.Z + offset.X * offset.Z;
        }
    }
}
